% a middle bridge between twisting controller and linear feedback controller
% run this and build main.tex to see main.pdf for document
params=struct();
tfinal=40;   
params.k=[2;6];
params.f=@(x,t)[sin(2*pi*t)];   
initial=[2;2];
[d0,s0]=rhs(0,initial,params);
profile=repmat(params,6,1);
profile(1).f=@(x,t)[sin(2*pi*t)];
profile(1).step=0.1;
profile(1).title="sin2pit_1e-1";

profile(2).f=@(x,t)[sin(2*pi*t)];
profile(2).step=0.001;
profile(2).title="sin2pit_1e-3";

profile(3).f=@(x,t)[1];
profile(3).step=0.1;
profile(3).title="const1_1e-1";

profile(4).f=@(x,t)[1];
profile(4).step=0.001;
profile(4).title="const1_1e-3";

profile(5).f=@(x,t)[sin(2*pi*t)];
profile(5).step=0.1;
profile(5).k=[2;1];
profile(5).title="sin2pit_twisting_1e-1";

profile(6).f=@(x,t)[sin(2*pi*t)];
profile(6).k=[2;1];
profile(6).step=0.001;
profile(6).title="sin2pit_twisting_1e-3";

for ip=1:6
    params=profile(ip);
    f=@(t,x)rhs(t,x,params);
    if ip>=5
        f=@(t,x)rhs_twist(t,x,params);
    end
    [t,x]=rk4(f,0,initial,30,params.step);
    s=repmat(s0,length(t),1);
    for i=1:length(t)
        [di,si]=f(t(i),x(i,:)');
        s(i)=si;
    end

    tiledlayout(2,1)
    nexttile;hold on;
    plot(t,x(:,1))
    plot(t,x(:,2))
    legend(["x","v"])
    grid on
    nexttile;hold on
    plot(t,horzcat(s.u))
    plot(t,horzcat(s.delta))
    legend(["u","\delta"],"Location","Southeast")
    grid on
    exportgraphics(gcf,"out/"+params.title+".png")
    
    % figure();
    % plot(x(:,1),x(:,2))
    % xlabel("x")
    % ylabel("v")
    % axis equal
    % grid on
    % exportgraphics(gcf,"out/"+params.title+"traj.png")
end

function [dxdt,s]=rhs(t,states,p)
    s=struct();
    x=states(1);
    v=states(2);
    s.u=-p.k(1)*sign(x)-p.k(2)*v;
    s.delta=p.f(x,t);
    dxdt=[v;s.u+s.delta];
end

function [dxdt,s]=rhs_twist(t,states,p)
    s=struct();
    x=states(1);
    v=states(2);
    s.u=-p.k(1)*sign(x)-p.k(2)*sign(v);
    s.delta=p.f(x,t);
    dxdt=[v;s.u+s.delta];
end